#ifndef CAROS_KUKA_TCP_CLIENT_H
#define CAROS_KUKA_TCP_CLIENT_H

#include <geometry_msgs/WrenchStamped.h>
#include <ros/ros.h>
#include <unistd.h>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <cstdlib>
#include <deque>
#include <fstream>
#include <iostream>
#include <rw/math.hpp>
#include <string>
#include <thread>  // NOLINT(build/c++11)

#define HEADER_LENGTH 4
#define MAX_MSG_LENGTH 2048

void default_set_state(std::string, void*);

typedef void (*input_handler)(std::string, void*);

class tcp_client
{
 private:
  static const int header_length = HEADER_LENGTH;
  static const int max_msg_length = MAX_MSG_LENGTH;
  boost::asio::io_service& io_service_;
  boost::asio::ip::tcp::socket socket_;
  boost::asio::streambuf buffer_;
  std::string read_msg_;
  char msg_len[HEADER_LENGTH];
  char msg_txt[MAX_MSG_LENGTH];
  int body_length_;
  std::deque<std::string> write_msgs_;
  bool running_;
  std::string terminate_connection_string;
  input_handler input_handle;
  void* v_state;

 public:
  tcp_client(boost::asio::io_service& io_service, boost::asio::ip::tcp::resolver::iterator endpoint_iterator,
             input_handler function = default_set_state, void* def_state = nullptr);
  ~tcp_client();
  void set_terminate_connection_string(std::string new_msg);
  bool running()
  {
    return running_;
  }

  void write(const std::string& msg);

  void close();


 private:
  void do_connect(boost::asio::ip::tcp::resolver::iterator endpoint_iterator);
  void do_read_header();
  void do_read_body();
  void do_write();
};
#endif  // CAROS_KUKA_TCP_CLIENT_H
